package sdp;

import java.lang.Math;

import javax.swing.JButton;
import javax.swing.JSlider;

/** This class provides an abstraction layer over the raw wheel speeds, by allowing a Strategy to simply specify a goal
 *  position and rotation, a direction of movement (forwards or backwards) and simple kick() method to perform kicks.
 */
public class Control {
	
	/** Construct giving a reference to the world. */
	public Control( World world, ControlPanel panel, Timer timer ) {
		this.world        = world;
		this.timer        = timer;
		this.panel        = panel;
		this.goalPosition = world.getPlayerState().getPosition();
		this.goalRotation = world.getPlayerState().getRotation();
		
		// Add some buttons
		this.panel.addButton("Kick!", new ControlPanel.ButtonCallback () {

			@Override
			public void buttonClicked(JButton b) {
				Control.this.kick();
			}
			
		});
		
		this.panel.addSlider("kick 1st t", (int)this.kickFirstTimestamp, 0, 10000, new ControlPanel.SliderCallback() {
			
			@Override
			public void valueChanged(JSlider s, int newValue) {
				Control.this.kickFirstTimestamp = (long)newValue;
				System.out.println(newValue);
				
			}
		});
		
	this.panel.addSlider("kick 2st t", (int)this.kickSecondTimestamp, 0, 10000, new ControlPanel.SliderCallback() {
			
			@Override
			public void valueChanged(JSlider s, int newValue) {
				Control.this.kickSecondTimestamp = (long)newValue;
				System.out.println(newValue);
			}
		});
		
	}
	
	/** Set the position to go to. */
	public void setGoalPosition( Vector2 newGoalPosition ) {
		goalPosition = newGoalPosition;
	}
	
	/** Set the robot's rotation to be achieved. */
	public void setGoalRotation( double newGoalRotation ) {
		goalRotation = capAngle( newGoalRotation );
	}
	
	/** Achieve the goal by going backwards or forwards. */
	public void setGoBackwards( boolean newGoBackwards ) {
		goBackwards = newGoBackwards;
	}
	
	/** Is a kicking action still going on? */
	public boolean isKicking() {
		assert false : "Kicking not implemented.";
		return false;
	}
	
	/** Perform kicking action. */
	public void kick() {
		
		if (this.kicking) {
			return;
		}
		
		this.initialKickTime = this.timer.getCurrentTime();
		
		this.kicking = true;
		this.kickInit = true;
		
	}
	
	/** Set the right wheel speeds to get to the goal. */
	public void update() {
		assert world.getMotorCount() == 3;
		
		Vector2 position = world.getPlayerState().getPosition();
		double  rotation = world.getPlayerState().getRotation();
		
		if( goBackwards )
			rotation = capAngle( rotation + Math.PI );
		
		double[] motorSpeeds  = { 0.0, 0.0, 0.0 };
		double   rotationDiff = angleDiff( goalRotation, rotation );
		double   forwardSpeed  = 0.0;
		
		// If we're not at our goal position yet, we need a forward speed to go in that direction.
		// We don't care that we may not be pointing the right direction yet, the rotation code
		// that follows will take care of that.
		if( !position.approxEquals( goalPosition, positionTolerance ) ) {
			Vector2 toGoal   = goalPosition.minus( position );
			double  distance = toGoal.computeNorm();
			
			forwardSpeed = Math.min( distance / wheelRadius, maxWheelSpeed / movementSlowDown ) * movementSlowDown;
			
			if( goBackwards )
				forwardSpeed = -forwardSpeed;
		}
		
		// relative wheel speed (right - left)
		double relWheelSpeed = rotationDiff / 2.0 * axleLength / wheelRadius / world.getTimeStep();
		double limit         = maxWheelSpeed / rotationSlowDown;
		
		// limit the relative wheel speed
		if( relWheelSpeed < - limit )
			relWheelSpeed = -limit;
		else if( relWheelSpeed > limit )
			relWheelSpeed = limit;
		
		// apply the slowdown factor
		relWheelSpeed *= rotationSlowDown;
		
		// set the wheel speeds to the forward speed, displaced by the relative velocity required for turning
		motorSpeeds[ 0 ] = forwardSpeed - relWheelSpeed;
		motorSpeeds[ 1 ] = forwardSpeed + relWheelSpeed;
		
		if (this.kicking) {
			
			
			
			if (this.kickInit) {
				
				motorSpeeds[2] = 10000;
				
				if (this.timer.didTimeElapse(this.initialKickTime, this.kickFirstTimestamp)) {
					motorSpeeds[2] = 0.0;
					this.kickInit = false;
					
					System.out.println("initial kicking phase");
				}
				
			}
			else {
				
				System.out.println("backing phase");
				
				motorSpeeds[2] = -1000;
				
				if (this.timer.didTimeElapse(this.initialKickTime, this.kickSecondTimestamp)) {
					motorSpeeds[2] = 0.0;
					this.kicking = false;
					
					System.out.println("kicking done");
				}
			}
			
		}
		else {
			motorSpeeds[2] = 0;
		}
		
		world.setMotorSpeeds( capWheelSpeeds( motorSpeeds ) );
	}
	
	/**
	 *  Shift wheel speeds to bring both of them in the [-maxWheelSpeed, +maxWheelSpeed].
	 *  Assumes abs(motorSpeeds[1]-motorSpeeds[0]) <= 2*maxWheelSpeed. This means they can be
	 *  shifted to fit.
	 */
	private double[] capWheelSpeeds( double[] motorSpeeds ) {
		double correction = 0.0;
		
		assert motorSpeeds[1] - motorSpeeds[0] <= (2 * maxWheelSpeed);
		
		if( motorSpeeds[ 0 ] > maxWheelSpeed )
			correction = motorSpeeds[ 0 ] - maxWheelSpeed;
		else if( motorSpeeds[ 0 ] < - maxWheelSpeed )
			correction = motorSpeeds[ 0 ] + maxWheelSpeed;
		else if( motorSpeeds[ 1 ] > maxWheelSpeed )
			correction = motorSpeeds[ 1 ] - maxWheelSpeed;
		else if( motorSpeeds[ 1 ] < - maxWheelSpeed )
			correction = motorSpeeds[ 1 ] + maxWheelSpeed;
		
		motorSpeeds[ 0 ] -= correction;
		motorSpeeds[ 1 ] -= correction;
		
		return motorSpeeds;
	}
	
	/// Bring an angle into the [0,2*pi) interval.
	private static double capAngle( double angle ) {
		return angle - ( 2.0 * Math.PI ) * Math.floor( angle / (2.0 * Math.PI) );
	}
	
	/// Compute an angle difference in the (-pi,pi] interval.
	/// Assumes abs( a - b ) <= 2*PI
	private static double angleDiff( double a, double b ) {
		assert Math.abs( a - b ) <= 2.0 * Math.PI;
		
		double r = a - b;
		
		if( r < - Math.PI )
			r += 2 * Math.PI;
		else if( r > Math.PI )
			r -= 2 * Math.PI;
		
		assert r >= -Math.PI;
		assert r <= Math.PI;
		
		return r;
	}
	
	private Timer			timer;
	private World   		world;
	private ControlPanel 	panel;
	private Vector2 		goalPosition;
	private double  		goalRotation;
	
	private long 			kickFirstTimestamp = 127;
	private long			kickSecondTimestamp = 349;
	
	private boolean goBackwards       = false;
	private double  positionTolerance = 0.1;
	private double  maxWheelSpeed     = 16.0;
	private double  movementSlowDown  = 1.0;
	private double  rotationSlowDown  = 0.25;
	
	// FIXME: Set to actual values
	private double  axleLength        = 17.0;
	private double  wheelRadius       = 3.0;
	
	// Variables used to execute a proper kick
	private boolean kicking           = false;
	private boolean kickInit;
	private double initialKickTacho   = 0.0;
	private long initialKickTime      = 0;
	
}
